from DebugTrace import DebugTrace

class MotorStub(object):
        def __init__(self,traceLevel=1,position=0):
                self.position=position
                self.minEndStop=None
                self.maxEndStop=None
                self.trace = DebugTrace("Motor",traceLevel)

        def addMinEndstop(self,position):
                self.minEndStop=float(position)
                if (self.position<position):
                        self.position=position
                self.trace.trace(2,"Min Endstop set at position "+str(position))
        
        def addMaxEndstop(self,position):
                self.maxEndStop=float(position)
                if (self.position>position):
                        self.position=position
                self.trace.trace(2,"Max Endstop set at position "+str(position))

        def getPosition(self):
                return self.position

        def setPosition(self,pos):
                self.trace.trace(2,"Current position changed from "+str(self.position)+" to "+str(pos))
                distance=self.getDistanceToPosition(float(pos))
                self.position=float(pos)
                if(self.minEndStop!=None):
                        self.minEndStop+=float(distance)
                if (self.maxEndStop!=None):
                        self.maxEndStop+=float(distance)
                

        def getDistanceToEndstop(self,minOrMax):
                if minOrMax == "min":
                        if (self.minEndStop!=None):
                                return self.getDistanceToPosition(self.minEndStop)
                        else:
                                self.trace.trace(1,"No min endstop to find distance to!")
                elif minOrMax == "max":
                        if (self.maxEndStop!=None):
                                return self.getDistanceToPosition(self.maxEndStop)
                        else:
                                self.trace.trace(1,"No max endstop to find distance to!")

        def getDistanceToPosition(self,pos):
                if (pos!=None):
                        return float(pos)-self.position
        
        def move(self,amount):
                if (amount == None):
                        return "ok"
                
                if (amount>0 and self.maxEndStop!=None and self.position+amount>self.maxEndStop):
                        self.position=self.maxEndStop
                        self.trace.trace(2,"Hit max endstop at "+str(self.position))
                elif (amount<0 and self.minEndStop!=None and self.position+amount<self.minEndStop):
                        self.position=self.minEndStop
                        self.trace.trace(2,"Hit min endstop at "+str(self.position))

                else:
                         self.position+=amount
                         self.trace.trace(2,"Moved by "+str(amount))


